#ifndef __IMU_H__
#define __IMU_H__

#include <vector>
#include <opencv2/core/core.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <sophus/se3.hpp>
#include <mutex>

using namespace cv;
using namespace std;

namespace ORBSLAM
{
    class ImuDataPoint
    {
    public:
        ImuDataPoint();
        // ~ImuDataPoint();
        ImuDataPoint(Point3f acc, Point3f gyro, double time_stamp);
        // ImuDataPoint(double a);

        ImuDataPoint(double ax, double ay, double az, double wx, double wy, double wz, double t);

        Eigen::Vector3f m_a;
        Eigen::Vector3f m_w;
        double m_t;
    };

    class PreIntegrationData
    {
    public:
        PreIntegrationData();
    };

    class Bias
    {
    public:
        Bias() : m_bax(0), m_bay(0), m_baz(0), m_bwx(0), m_bwy(0), m_bwz(0) {}
        Bias(float bax, float bay, float baz, float bwx, float bwy, float bwz) : m_bax(bax), m_bay(bay), m_baz(baz), m_bwx(bwx), m_bwy(bwy), m_bwz(bwz) {}
        float m_bax;
        float m_bay;
        float m_baz;
        float m_bwx;
        float m_bwy;
        float m_bwz;
    };

    class Extrinsics
    {
    public:
        Extrinsics();
    };

    class PreIntegrater
    {
    public:
        PreIntegrater();
        PreIntegrationData Pre_integrate(PreIntegrationData pre_integration_data, double last_frame_timestamp, Bias last_frame_bias, vector<ImuDataPoint> v_imu_data, double cur_frame_timestamp);

        double m_time_base;
        Bias m_bias;
    };
}
#endif // __IMU_H__